#include "osa-control.h"

#include <sstream>
#include <iostream>

//swap higher order and lower order bytes
void swap_float(float *y,unsigned length)
{
  char temp;

  char *c=(char*)y;
  for (unsigned i=0; i<length; i++,c+=4)
    {
      // Swap bit 0 and 3
      temp = c[0];
      c[0]=c[3];
      c[3]=temp;

      // Swap bit 1 and 2
      temp = c[1];
      c[1]=c[2];
      c[2]=temp;
    }
}

osa_control::osa_control(std::string device_ip,std::string device_name)
{
  this->device_ip = device_ip;
  this->device_name = device_name;

  char *pb = this->trace_buffer;
  for (unsigned long i=0; i<OSA_BUFFER_LEN; i++,pb++)
    *pb=0;

  this->status=0;	// Default is no error.

  connect();
}

void osa_control::check_status(std::string message)
{
  if (status >= 0) return;

  std::stringstream format_message;
  format_message << '[' << status << ']' << message << std::endl
  << "DEBUG: ";
  switch (-status)
    {
    case 1:
      format_message << "syntax error";
      break;
    case 3:
      format_message << "device not accessible";
      break;
    case 4:
      format_message << "invalid link identifier";
      break;
    case 5:
      format_message <<"parameter error";
      break;
    case 6:
      format_message <<"channel not established";
      break;
    case 8:
      format_message << "operation not supported";
      break;
    case 9:
      format_message << "out of resources";
      break;
      break;
    case 11:
      format_message << "device locked by another link";
      break;
    case 12:
      format_message << "no lock held by this link";
      break;
    case 15:
      format_message << "I/O timeout";
      break;
    case 17:
      format_message << "I/O error";
      break;
    case 21:
      format_message << "invalid address";
      break;
    case 23:
      format_message << "abort";
      break;
    case 29:
      format_message << "channel already established";
    }
  throw format_message.str();
}


void osa_control::connect()
{
  status=vxi11_open_device((char*)device_ip.c_str(),&clink,(char*)device_name.c_str());
  check_status("Could not connect device");
}
osa_control::~osa_control()
{

//Turn back on the display

status = vxi11_send(&clink, "DISP ON");
check_status("Could not send instruction (TURN DISPLAY ON)");


  status=vxi11_close_device((char*)device_ip.c_str(),&clink);
  check_status("Could not disconnect device");
}

void osa_control::initialize(unsigned wavelength_start, unsigned wavelength_stop, float sensitivity,float ref_level, float bw_res, unsigned trace_no)
{

  this->trace_no = trace_no;
  this->wavelength_start = wavelength_start;
  this->wavelength_stop = wavelength_stop;
  this->resolution = resolution;
  this->sensitivity = sensitivity;

  char temp[2];

//status = vxi11_send(&clink, "*RST;*OPC?");
//check_status("Could not send instruction (Preset device)");

//status = vxi11_receive(&clink, temp, 2,timeout);
//check_status("Could not receive reply (Preset device)");

//=====================================================

  status = vxi11_send(&clink, "SYST:COMM:GPIB:BUFF ON");
  check_status("Could not send instruction (TURN COMMAND BUFFER ON)");

  status = vxi11_send(&clink, "INIT:CONT OFF");
  check_status("Could not send instruction (SET SINGLE SWEEP MODE)");

status = vxi11_send(&clink, "DISP OFF");
check_status("Could not send instruction (TURN DISPLAY OFF)");

  status = vxi11_send(&clink, "CALC:MARK1:STAT OFF");
  check_status("Could not send instruction (TURN MARKER OFF)");

#ifdef __VXI11_ASCII__
  status = vxi11_send(&clink, "FORM ASCII");
  check_status("Could not send instruction (SET TRACE DATA FORMAT TO ASCII)");
#else
  status = vxi11_send(&clink, "FORM REAL,32");
  check_status("Could not send instruction (SET TRACE DATA FORMAT TO REAL32)");
#endif

  std::stringstream format_message;

  format_message << "SENS:SWE:POINT " << trace_no;

  status = vxi11_send(&clink, format_message.str().c_str());
  check_status("Could not send instruction (SET TRACE LENGTH)");

  format_message.str("");	//reset
  format_message
  << "STAR " << wavelength_start
  << " NM;STOP " << wavelength_stop << " NM";

  status = vxi11_send(&clink, format_message.str().c_str());
  check_status("Could not send instruction (SET WAVELENGTH)");


  format_message.str("");	//reset
  format_message
  << "BAND " << bw_res << " NM";

  status = vxi11_send(&clink, format_message.str().c_str());
  check_status("Could not send instruction (SET resolution bandwidth)");

  format_message.str("");	//reset
  format_message
  << "POW:RANG:LOW " << sensitivity << " DBM";

  status = vxi11_send(&clink, format_message.str().c_str());
  check_status("Could not send instruction (SET SENSITIVITY)");

  format_message.str("");	//reset
  format_message
  << "DISP:TRAC:Y:RLEV " << ref_level << " DBM";

  status = vxi11_send(&clink, format_message.str().c_str());
  check_status("Could not send instruction (SET REF_LEVEL");

}

void osa_control::get_trace(float *y, unsigned length)
{

  char temp[2];

  if (length != trace_no)
    throw "Cannot get trace (dimension mismatch)";

  status = vxi11_send(&clink, "INIT;*WAI;TRAC? TRA");
  check_status("Could not send instruction (REQUEST DATA FROM TRACE A)");

  status = vxi11_receive(&clink, trace_buffer, OSA_BUFFER_LEN,OSA_TIMEOUT);

#ifdef __VXI11_ASCII__
  std::stringstream ascii_float(trace_buffer);

  float *py=y;
  char comma;
  for (unsigned i=0; i<trace_no; i++,py++)
    ascii_float >> *py >> comma;

#else
  char * c = trace_buffer;

  if ( c[0] != '#')
    throw "Received string is not binary (no sharp)";

  unsigned magnitude=0;
  for (unsigned ten=1; ten<=length; ten*=10,magnitude++);

  if ( c[1]-'0' != magnitude )
    throw "Received string is not binary (wrong magnitude)";

  unsigned data_length = 0;
  for (unsigned i=2; i<=5; i++)
    data_length = data_length *10 + (c[i] - '0');
  data_length /=4;

  if ( data_length != length)
    throw "Received string is not binary (wrong length)";

  float *pf = (float*) (c+6);
  float *py = y;
  for (unsigned i=0; i<length; i++,pf++,py++)
    *py = *pf;

  swap_float(y,length);
#endif
}


